﻿using System;
using MoveFramework_CS;
using System.Threading;

namespace MoveModel
{
    public class Move
    {
		#region Public variables

		public int Trigger {
			get {
				return trigger;
			}
			set {
				trigger = value;
			}
		}

		int trigger = 0;
        MoveWrapper.Quaternion or;

		public float Mx {
			get {
                return or.x;
			}
		}

		public float My {
			get {
                return or.y;
			}
		}

		public float Mz {
			get {
                return or.z;
			}
		}

        public float Mw
        {
            get
            {
                return or.w;
            }
        }

        MoveWrapper.Vector3 angAcc;

        public float Ax
        {
            get
            {
                return angAcc.x;
            }
        }

        public float Ay
        {
            get
            {
                return angAcc.y;
            }
        }

        public float Az
        {
            get
            {
                return angAcc.z;
            }
        }

        MoveWrapper.Vector3 pos;

		public double Radius {
			get {
                return applyCalibration(pos.z * 5 - 127.5);
			}
		}

        private double calibrationMin = 0.0;
        public void setCalibrationMin()
        {
            calibrationMin = this.Radius;
        }
        private double calibrationMax = 0.0;
        public void setCalibrationMax()
        {
            calibrationMax = this.Radius;
        }
        public bool calibrationDone = false;

        private double applyCalibration(double z)
        {
            if (!calibrationDone) return z;
            return 200 / (calibrationMax - calibrationMin) * (z - calibrationMax) + 100;
            //(100 - (-100)) / (max - min) * (z - max) + 100;
        }

        private double applyXYCalibration()
        {
            if (!calibrationDone) return 1;
            return (100 - (-100)) / (calibrationMax - calibrationMin);
        }

        //TODO: TRY IT OUT!!!!
        public double X
        {
            get
            {
                //return (int) ((255 - Radius) * (pos.x + 180) / 180);
                return Math.Round(pos.x * 5, 2) * applyXYCalibration();
            }
        }

        public double Y {
            get
            {
                //return (int)((255 - Radius) * (pos.y + 180) / 180);
                return Math.Round(pos.y * 5, 2) * applyXYCalibration();
            }
        }		

		public int Lr {
			get {
				return lr;
			}
			set {
				lr = value;
			}
		}

		public int Lg {
			get {
				return lg;
			}
			set {
				lg = value;
			}
		}

		public int Lb {
			get {
				return lb;
			}
			set {
				lb = value;
			}
		}

		int lr, lg, lb;
		private bool isShouldRender;

		public bool IsShouldRender {
			get {
				return isShouldRender;
			}
			set {
				isShouldRender = value;
			}
		}

		public bool IsTracking {
			get {
                return true; // tracker.GetStatus(move) == Status.Tracker_TRACKING;
			}
		}


		private bool isMoveButton = false;

		public bool IsMoveButton {
			get { 
				return isMoveButton; 
			}
		}

		private bool startDrawing = false;

        private bool isPSButton;

        public bool IsPSButton
        {
            get { return isPSButton; }
        }
        
		public bool StartDrawing {
			get {
				return startDrawing;
			}
		}

        private int btnHoldTimerX = 0;
		private bool isCrossBtn = false;

		public bool IsCrossBtn {
            get
            {
                if (isCrossBtn)
                    if (btnHoldTimerX == 0)
                    {
                        btnHoldTimerX++;
                        return true;
                    }
                    else
                    {
                        if (btnHoldTimerX > 10)
                        {
                            return true;
                        }
                        btnHoldTimerX++;
                        return false;
                    }
                else
                {
                    btnHoldTimerX = 0;
                    return false;
                }
            }
		}

        private int btnHoldTimerC = 0;
		private bool isCircleBtn = false;

		public bool IsCircleBtn {
			get {
                if (isCircleBtn )
                    if (btnHoldTimerC == 0)
                    {
                        btnHoldTimerC++;
                        return true;
                    }
                    else
                    {
                        if (btnHoldTimerC > 10)
                        {                            
                            return true;
                        }
                        btnHoldTimerC++;
                        return false;
                    }
                else
                {
                    btnHoldTimerC = 0;
                    return false;
                }
			}
		}

		private bool isSelectBtn = false;

		public bool IsSelectBtn {
			get {
				return isSelectBtn;
			}
		}

        private int btnHoldTimerS = 0;
		private bool isSquareBtn = false;

		public bool IsSquareBtn {
			get {
                if (isSquareBtn)
                    if (btnHoldTimerS == 0)
                    {
                        btnHoldTimerS++;
                        return true;
                    }
                    else
                    {
                        if (btnHoldTimerS > 10)
                        {                            
                            return true;
                        }
                        btnHoldTimerS++;
                        return false;
                    }
                else
                {
                    btnHoldTimerS = 0;
                    return false;
                }
			}
		}

        private int btnHoldTimerT = 0;
		private bool isTriangleBtn = false;

		public bool IsTriangleBtn {
			get {
				
                if (isTriangleBtn )
                    if (btnHoldTimerT == 0)
                    {
                        btnHoldTimerT++;
                        return true;
                    }
                    else
                    {
                        if (btnHoldTimerT > 10)
                        {                            
                            return true;
                        }
                        btnHoldTimerT++;
                        return false;
                    }
                else
                {
                    btnHoldTimerT = 0;
                    return false;
                }
			}
		}

		private bool isStartBtn = false;

		public bool IsStartBtn {
			get {
				return isStartBtn;
			}
		}

		#endregion

		public Move()
		{
            MoveWrapper.init();

            int moveCount = MoveWrapper.getMovesCount();

            if (moveCount > 0)
            {
                pos = MoveWrapper.getPosition(0);
                or = MoveWrapper.getOrientation(0);
                angAcc = new MoveWrapper.Vector3();
                angAcc.x = 0;
                angAcc.y = 0;
                angAcc.z = 0;
                angAcc = MoveWrapper.getAngularAcceleration(0);
                trigger = MoveWrapper.getTriggerValue(0);
                MoveWrapper.setRumble(0, 255);
                Thread.Sleep(40);
                MoveWrapper.setRumble(0, 0);
            }
            else
            {
                Console.WriteLine("No move controller detected");
                Console.ReadKey();
                System.Environment.Exit(0);
            }

            MoveWrapper.subscribeMoveUpdate(MoveUpdateCallback, MoveKeyDownCallback, MoveKeyUpCallback, NavUpdateCallback, NavKeyDownCallback, NavKeyUpCallback);
			isShouldRender = true;
		}

        ~Move()
        {
            MoveWrapper.unsubscribeMove();
        }

		public static void Main ()
		{
			new Move();
		}

        /*
		public void MoveLoop ()
		{
			tracker = new MoveTracker (move);
//			Random rnd = new Random();
			while (true) {
				tracker.Update ();
				UpdateMove ();
				if (tracker.GetStatus (move) == Status.Tracker_TRACKING) {
					isShouldRender = true;
				} else
					isShouldRender = false;

				//Thread.Sleep (5);
			}
		}
         */

        private ReaderWriterLock rwLock = new ReaderWriterLock();

        private MoveWrapper.Vector3 prevPos = new MoveWrapper.Vector3();

        void MoveUpdateCallback(int id, MoveWrapper.Vector3 position, MoveWrapper.Quaternion orientation, MoveWrapper.Vector3 angularAcceleration, int t)
        {
            

            //rwLock.AcquireWriterLock(1000);

            //if (prevPos.Equals(position))
            //{
            //    Console.WriteLine("Cannot see the MOVE Controller");
            //    return;
            //}
            prevPos = pos;


            trigger = t;
            MoveWrapper.setRumble(id, Trigger);
            or = orientation;
            pos = position;
            angAcc = angularAcceleration;

            //Console.WriteLine("angAcc: " + angAcc.x + " " + angAcc.y + " " + angAcc.z);

            startDrawing = Trigger > 0;
            
            isMoveButton = MoveWrapper.getButtonState(id, MoveButton.B_MOVE);
            isCrossBtn = MoveWrapper.getButtonState(id, MoveButton.B_CROSS);
            isCircleBtn = MoveWrapper.getButtonState(id, MoveButton.B_CIRCLE);
            isSquareBtn = MoveWrapper.getButtonState(id, MoveButton.B_SQUARE);
            isTriangleBtn = MoveWrapper.getButtonState(id, MoveButton.B_TRIANGLE);
            isSelectBtn = MoveWrapper.getButtonState(id, MoveButton.B_SELECT);
            isStartBtn = MoveWrapper.getButtonState(id, MoveButton.B_START);
            isPSButton = MoveWrapper.getButtonState(id, MoveButton.B_PS);

            //Console.WriteLine(isMoveButton + " " + isCrossBtnReleased + " " + isSelectBtnReleased + " " + isStartBtnReleased);

            //rwLock.ReleaseWriterLock();

            //Console.WriteLine("UPDATE moveid: " + id +
            //        " position: " + X + " " + Y + " " + Radius +
            //        " orientation:" + or.w + " " + or.x + " " + or.y + " " + or.z + " trigger: " + trigger);
        }

        //callback methods not working in this framework version
        void MoveKeyUpCallback(int id, int keyCode) { }
        void MoveKeyDownCallback(int id, int keyCode) { }
        void NavUpdateCallback(int id, int trigger1, int trigger2, int stickX, int stickY) { }
        void NavKeyUpCallback(int id, int keyCode) { }
        void NavKeyDownCallback(int id, int keyCode) { }




		//private ReaderWriterLock rwLock = new ReaderWriterLock ();

        /*
		public void UpdateMove ()
		{
			while (move.poll() != 0) {


				rwLock.AcquireWriterLock (1000);
				
				trigger = move.get_trigger ();				
				move.set_rumble (trigger);

				move.get_magnetometer_vector (out mx, out my, out mz);
				if (tracker.GetStatus (move) == Status.Tracker_TRACKING) {
					tracker.GetPosition (move, out x, out y, out radius);
					//Console.WriteLine (x + " " + y + " " + radius);
				} else
					Console.WriteLine ("Not tracking");

				uint pressed = 0, released = 0;
				move.get_button_events(out pressed, out released);
//				int button = move.get_buttons ();
//				if (button == (int)io.thp.psmove.Button.Btn_CIRCLE)
//					lr = (int)((mx + 1) * 127.5);
//				else if (button == (int)io.thp.psmove.Button.Btn_TRIANGLE)
//					lg = (int)((mx + 1) * 127.5);
//				else if (button == (int)io.thp.psmove.Button.Btn_CROSS)
//					lb = (int)((mx + 1) * 127.5);
				
//				if(button != 0)
//					win.SetColor(lr,lg,lb);
				if (pressed == (int)io.thp.psmove.Button.Btn_MOVE)
					isMoveButton = true;
				if (released == (int)io.thp.psmove.Button.Btn_MOVE)
					isMoveButton = false;
				if (pressed == (int)io.thp.psmove.Button.Btn_T)
					startDrawing = true;
				if (released == (int)io.thp.psmove.Button.Btn_T)
					startDrawing = false;
				isCrossBtnReleased = pressed == (int)io.thp.psmove.Button.Btn_CROSS;
				isCircleBtnReleased = pressed == (int)io.thp.psmove.Button.Btn_CIRCLE;
				isSelectBtnReleased = pressed == (int)io.thp.psmove.Button.Btn_SELECT;
				isSquareBtnReleased = pressed == (int)io.thp.psmove.Button.Btn_SQUARE;
				isTriangleBtnReleased = pressed == (int)io.thp.psmove.Button.Btn_TRIANGLE;
				isStartBtnReleased = pressed == (int)io.thp.psmove.Button.Btn_START;
				rwLock.ReleaseWriterLock ();

				move.update_leds ();
//				Console.WriteLine (lr + " " + lg + " " + lb);
			}
        
		}
         * */
    }
}
